#ifndef __RELOCALISER_SCARF_H
#define __RELOCALISER_SCARF_H
#include <TooN/se3.h>
#include <gvars3/instances.h>
#include <fstream>
#include "ATANCamera.h"
#include "PoseEstimator.h"

#include "Map.h"
#include "MapPoint.h"
#include "Relocaliser.h"
#include <cvd/synchronized.h>
#include <set>

class RelocaliserSCARF : public Relocaliser
{
public:
  RelocaliserSCARF(const ATANCamera &camera);
  ~RelocaliserSCARF();

  bool AttemptRecovery(Map *pMap, boost::shared_ptr<KeyFrame> pCurrentKF, boost::shared_ptr<KeyFrame> pMapKF);
  SE3<> BestPose();
  std::string Type() { return "SCARF"; };

  std::vector<Observation> mvObservations;
  std::vector<std::vector<Observation*> > mvClusters;
  std::vector<PoseHypothesis> mvRelocaliserPose;

private:
  ATANCamera mCamera;
  PoseEstimator mPoseEstimator;
  boost::shared_ptr<KeyFrame> mpOldMapKF;
  SE3<> mse3Best;

  uint16_t DistanceSAD(uint8_t desc1[64], uint8_t desc2[64]);

  cv::flann::Index *mFlannIndex;
  cv::Mat *mFlannDescriptors;
  cv::Mat *mFlannWorldPositions;
  std::set<boost::shared_ptr<MapPoint> > msLocalMapPoints;

  const unsigned int mnDescriptorDimension;
  bool mbUsingGlobalMap;
  unsigned int mnMaxImageFeatures;
  unsigned int mnMaxLocalKeyFrames;
  unsigned int mnMaxLocalMapPoints;
  unsigned int mnRansacMinObservations;
  unsigned int mnRansacMaxHypotheses;
  unsigned int mnRansacMaxBlocks;
  unsigned int mnRansacGroupsPerBlock;
  unsigned int mnRansacGroupSize;
  double mdDbscanEps;
  int mnDbscanMinObs;
  unsigned int mnStaleMapCount;
  unsigned int mnStaleMapFrames;
  int mnGlobalSE3;      // Should we use provided external position estimate (absolute measurements from GPS)
  int mnGlobalSO3;      // Should we use provided external attitude estimate (absolute measurements from compass, gyros etc.)
  double mdMaxScarfSAD;
  double mdMaxScarfSSD;
  GVars3::gvar3<int> mgvnMaxImageFeatures;
  GVars3::gvar3<int> mgvnMaxLocalKeyFrames;
  GVars3::gvar3<int> mgvnMaxLocalMapPoints;
  GVars3::gvar3<int> mgvnRansacMinObservations;
  GVars3::gvar3<int> mgvnRansacMaxHypotheses;
  GVars3::gvar3<int> mgvnRansacMaxBlocks;
  GVars3::gvar3<int> mgvnRansacGroupsPerBlock;
  GVars3::gvar3<int> mgvnRansacGroupSize;
  GVars3::gvar3<double> mgvdDbscanEps;
  GVars3::gvar3<int> mgvnDbscanMinObs;
  GVars3::gvar3<int> mgvnStaleMapFrames;
  GVars3::gvar3<int> mgvnGlobalSE3;
  GVars3::gvar3<int> mgvnGlobalSO3;
  GVars3::gvar3<double> mgvdMaxScarfSAD;
  GVars3::gvar3<double> mgvdMaxScarfSSD;

  std::ofstream ofsLogger;
};

#endif









